#!/usr/bin/env python3

import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

def video_publisher():
    # 初始化ROS节点
    rospy.init_node('video_publisher', anonymous=True)
    
    # 创建发布者，发布到 /usb_cam2/image_raw 话题
    image_pub = rospy.Publisher('/usb_cam/image_raw', Image, queue_size=10)
    
    # 创建CvBridge对象
    bridge = CvBridge()
    
    # 打开视频文件
    video_path = '/home/firefly/Videos/摄像头/2025-05-08-151716.webm'
    cap = cv2.VideoCapture(video_path)
    
    if not cap.isOpened():
        rospy.logerr(f"Failed to open video file: {video_path}")
        return
    
    # 设置发布频率
    rate = rospy.Rate(30)  # 30帧每秒
    
    while not rospy.is_shutdown():
        ret, frame = cap.read()
        if not ret:
            rospy.loginfo("End of video file reached or failed to read frame.")
            break
        
        try:
            # 将OpenCV图像转换为ROS Image消息
            ros_image = bridge.cv2_to_imgmsg(frame, encoding="bgr8")
            # 发布消息
            image_pub.publish(ros_image)
            # cv2.imshow("aaa", frame)
            cv2.waitKey(1)
        except Exception as e:
            rospy.logerr(f"Error converting or publishing frame: {e}")
        
        # 按照设定频率休眠
        rate.sleep()
    
    # 释放视频资源
    cap.release()

if __name__ == '__main__':
    try:
        video_publisher()
    except rospy.ROSInterruptException:
        pass